#ifndef TEB_OBSTACLES_H
#define TEB_OBSTACLES_H

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <Eigen/Geometry>
#include <complex>
#include <boost/shared_ptr.hpp>
#include <boost/pointer_cast.hpp>
#include "distance_calculations.h"
#include "teb_types.h"

//#include <geometry_msgs/Polygon.h>
//#include <geometry_msgs/TwistWithCovariance.h>
//#include <geometry_msgs/QuaternionStamped.h>

namespace teb_local_planner
{

/**
 * @class Obstacle
 * @brief Abstract class that defines the interface for modelling obstacles
 */
    class Obstacle_teb
    {
    public:

        /**
          * @brief Default constructor of the abstract obstacle class
          */
        Obstacle_teb() : dynamic_(false), centroid_velocity_(Eigen::Vector2d::Zero())
        {
        }

        /**
         * @brief Virtual destructor.
         */
        virtual ~Obstacle_teb()
        {
        }


        /** @name Centroid coordinates (abstract, obstacle type depending) */
        //@{

        /**
          * @brief Get centroid coordinates of the obstacle
          * @return Eigen::Vector2d containing the centroid
          */
        virtual const Eigen::Vector2d& getCentroid() const = 0;

        /**
          * @brief Get centroid coordinates of the obstacle as complex number
          * @return std::complex containing the centroid coordinate
          */
        virtual std::complex<double> getCentroidCplx() const = 0;

        //@}


        /** @name Collision checking and distance calculations (abstract, obstacle type depending) */
        //@{

        /**
          * @brief Check if a given point collides with the obstacle
          * @param position 2D reference position that should be checked
          * @param min_dist Minimum distance allowed to the obstacle to be collision free
          * @return \c true if position is inside the region of the obstacle or if the minimum distance is lower than min_dist
          */
        virtual bool checkCollision(const Eigen::Vector2d& position, double min_dist) const = 0;

        /**
          * @brief Check if a given line segment between two points intersects with the obstacle (and additionally keeps a safty distance \c min_dist)
          * @param line_start 2D point for the end of the reference line
          * @param line_end 2D point for the end of the reference line
          * @param min_dist Minimum distance allowed to the obstacle to be collision/intersection free
          * @return \c true if given line intersects the region of the obstacle or if the minimum distance is lower than min_dist
          */
        virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const = 0;

        /**
          * @brief Get the minimum euclidean distance to the obstacle (point as reference)
          * @param position 2d reference position
          * @return The nearest possible distance to the obstacle
          */
        virtual double getMinimumDistance(const Eigen::Vector2d& position) const = 0;

        /**
         * @brief Get the minimum euclidean distance to the obstacle (line as reference)
         * @param line_start 2d position of the begin of the reference line
         * @param line_end 2d position of the end of the reference line
         * @return The nearest possible distance to the obstacle
         */
        virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const = 0;

        /**
         * @brief Get the minimum euclidean distance to the obstacle (polygon as reference)
         * @param polygon Vertices (2D points) describing a closed polygon
         * @return The nearest possible distance to the obstacle
         */
        virtual double getMinimumDistance(const Point2dContainer& polygon) const = 0;

        /**
         * @brief Get the closest point on the boundary of the obstacle w.r.t. a specified reference position
         * @param position reference 2d position
         * @return closest point on the obstacle boundary
         */
        virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const = 0;

        //@}



        /** @name Velocity related methods for non-static, moving obstacles */
        //@{

        /**
          * @brief Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (point as reference)
          * @param position 2d reference position
          * @param t time, for which the minimum distance to the obstacle is estimated
          * @return The nearest possible distance to the obstacle at time t
          */
        virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const = 0;

        /**
          * @brief Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (line as reference)
          * @param line_start 2d position of the begin of the reference line
          * @param line_end 2d position of the end of the reference line
          * @param t time, for which the minimum distance to the obstacle is estimated
          * @return The nearest possible distance to the obstacle at time t
          */
        virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const = 0;

        /**
          * @brief Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (polygon as reference)
          * @param polygon Vertices (2D points) describing a closed polygon
          * @param t time, for which the minimum distance to the obstacle is estimated
          * @return The nearest possible distance to the obstacle at time t
          */
        virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const = 0;

        /**
          * @brief Predict position of the centroid assuming a constant velocity model
          * @param[in]  t         time in seconds for the prediction (t>=0)
          * @param[out] position  predicted 2d position of the centroid
          */
        virtual void predictCentroidConstantVelocity(double t, Eigen::Ref<Eigen::Vector2d> position) const
        {
            position = getCentroid() + t * getCentroidVelocity();
        }

        /**
          * @brief Check if the obstacle is a moving with a (non-zero) velocity
          * @return \c true if the obstacle is not marked as static, \c false otherwise
          */
        bool isDynamic() const {return dynamic_;}

        /**
          * @brief Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid
          * @remarks Setting the velocity using this function marks the obstacle as dynamic (@see isDynamic)
          * @param vel 2D vector containing the velocities of the centroid in x and y directions
          */
        void setCentroidVelocity(const Eigen::Ref<const Eigen::Vector2d>& vel) {centroid_velocity_ = vel; dynamic_=true;}

        /**
          * @brief Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid
          * @remarks Setting the velocity using this function marks the obstacle as dynamic (@see isDynamic)
          * @param velocity geometry_msgs::TwistWithCovariance containing the velocity of the obstacle
          * @param orientation geometry_msgs::QuaternionStamped containing the orientation of the obstacle
          */
        void setCentroidVelocity(const TwistWithCovariance& velocity,
                                 const Quaternion& orientation)
        {
            // Set velocity, if obstacle is moving
            Eigen::Vector2d vel;
            vel.coeffRef(0) = velocity.twist.linear.x();
            vel.coeffRef(1) = velocity.twist.linear.y();

            // If norm of velocity is less than 0.001, consider obstacle as not dynamic
            // TODO: Get rid of constant
            if (vel.norm() < 0.001)
                return;

            // currently velocity published by stage is already given in the map frame
//    double yaw = tf::getYaw(orientation.quaternion);
//    ROS_INFO("Yaw: %f", yaw);
//    Eigen::Rotation2Dd rot(yaw);
//    vel = rot * vel;
            setCentroidVelocity(vel);
        }

        void setCentroidVelocity(const TwistWithCovariance& velocity,
                                 const QuaternionStamped& orientation)
        {
            setCentroidVelocity(velocity, orientation.quaternion);
        }

        /**
          * @brief Get the obstacle velocity (vx, vy) (w.r.t. to the centroid)
          * @returns 2D vector containing the velocities of the centroid in x and y directions
          */
        const Eigen::Vector2d& getCentroidVelocity() const {return centroid_velocity_;}

        //@}



        /** @name Helper Functions */
        //@{

        /**
         * @brief Convert the obstacle to a polygon message
         *
         * Convert the obstacle to a corresponding polygon msg.
         * Point obstacles have one vertex, lines have two vertices
         * and polygons might are implictly closed such that the start vertex must not be repeated.
         * @param[out] polygon the polygon message
         */
        virtual void toPolygonMsg(Polygon& polygon) = 0;

        virtual void toTwistWithCovarianceMsg(TwistWithCovariance& twistWithCovariance)
        {
            if (dynamic_)
            {
                twistWithCovariance.twist.linear.x() = centroid_velocity_(0);
                twistWithCovariance.twist.linear.y() = centroid_velocity_(1);
            }
            else
            {
                twistWithCovariance.twist.linear.x() = 0;
                twistWithCovariance.twist.linear.y() = 0;
            }

            // TODO:Covariance
        }

        //@}

    protected:

        bool dynamic_; //!< Store flag if obstacle is dynamic (resp. a moving obstacle)
        Eigen::Vector2d centroid_velocity_; //!< Store the corresponding velocity (vx, vy) of the centroid (zero, if _dynamic is \c true)

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };


//! Abbrev. for shared obstacle pointers
    typedef boost::shared_ptr<Obstacle_teb> ObstaclePtr;
//! Abbrev. for shared obstacle const pointers
    typedef boost::shared_ptr<const Obstacle_teb> ObstacleConstPtr;
//! Abbrev. for containers storing multiple obstacles
    typedef std::vector<ObstaclePtr> ObstContainer;



/**
 * @class PointObstacle
 * @brief Implements a 2D point obstacle
 */
    class PointObstacle : public Obstacle_teb
    {
    public:

        /**
          * @brief Default constructor of the point obstacle class
          */
        PointObstacle() : Obstacle_teb(), pos_(Eigen::Vector2d::Zero())
        {}

        /**
          * @brief Construct PointObstacle using a 2d position vector
          * @param position 2d position that defines the current obstacle position
          */
        PointObstacle(const Eigen::Ref< const Eigen::Vector2d>& position) : Obstacle_teb(), pos_(position)
        {}

        /**
          * @brief Construct PointObstacle using x- and y-coordinates
          * @param x x-coordinate
          * @param y y-coordinate
          */
        PointObstacle(double x, double y) : Obstacle_teb(), pos_(Eigen::Vector2d(x,y))
        {}


        // implements checkCollision() of the base class
        virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
        {
            return getMinimumDistance(point) < min_dist;
        }


        // implements checkLineIntersection() of the base class
        virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
        {
            // Distance Line - Circle
            // refer to http://www.spieleprogrammierer.de/wiki/2D-Kollisionserkennung#Kollision_Kreis-Strecke
            Eigen::Vector2d a = line_end-line_start; // not normalized!  a=y-x
            Eigen::Vector2d b = pos_-line_start; // b=m-x

            // Now find nearest point to circle v=x+a*t with t=a*b/(a*a) and bound to 0<=t<=1
            double t = a.dot(b)/a.dot(a);
            if (t<0) t=0; // bound t (since a is not normalized, t can be scaled between 0 and 1 to parametrize the line
            else if (t>1) t=1;
            Eigen::Vector2d nearest_point = line_start + a*t;

            // check collision
            return checkCollision(nearest_point, min_dist);
        }


        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Eigen::Vector2d& position) const
        {
            return (position-pos_).norm();
        }

        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
        {
            return distance_point_to_segment_2d(pos_, line_start, line_end);
        }

        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Point2dContainer& polygon) const
        {
            return distance_point_to_polygon_2d(pos_, polygon);
        }

        // implements getMinimumDistanceVec() of the base class
        virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
        {
            return pos_;
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
        {
            return (pos_ + t*centroid_velocity_ - position).norm();
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
        {
            return distance_point_to_segment_2d(pos_ + t*centroid_velocity_, line_start, line_end);
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
        {
            return distance_point_to_polygon_2d(pos_ + t*centroid_velocity_, polygon);
        }

        // implements predictCentroidConstantVelocity() of the base class
        virtual void predictCentroidConstantVelocity(double t, Eigen::Ref<Eigen::Vector2d> position) const
        {
            position = pos_ + t*centroid_velocity_;
        }

        // implements getCentroid() of the base class
        virtual const Eigen::Vector2d& getCentroid() const
        {
            return pos_;
        }

        // implements getCentroidCplx() of the base class
        virtual std::complex<double> getCentroidCplx() const
        {
            return std::complex<double>(pos_[0],pos_[1]);
        }

        // Accessor methods
        const Eigen::Vector2d& position() const {return pos_;} //!< Return the current position of the obstacle (read-only)
        Eigen::Vector2d& position() {return pos_;} //!< Return the current position of the obstacle
        double& x() {return pos_.coeffRef(0);} //!< Return the current x-coordinate of the obstacle
        const double& x() const {return pos_.coeffRef(0);} //!< Return the current y-coordinate of the obstacle (read-only)
        double& y() {return pos_.coeffRef(1);} //!< Return the current x-coordinate of the obstacle
        const double& y() const {return pos_.coeffRef(1);} //!< Return the current y-coordinate of the obstacle (read-only)

        // implements toPolygonMsg() of the base class
        virtual void toPolygonMsg(Polygon& polygon)
        {
            polygon.points.resize(1);
            polygon.points.front().x = pos_.x();
            polygon.points.front().y = pos_.y();
            polygon.points.front().z = 0;
        }

    protected:

        Eigen::Vector2d pos_; //!< Store the position of the PointObstacle


    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

/**
 * @class CircularObstacle
 * @brief Implements a 2D circular obstacle (point obstacle plus radius)
 */
    class CircularObstacle : public Obstacle_teb
    {
    public:

        /**
          * @brief Default constructor of the circular obstacle class
          */
        CircularObstacle() : Obstacle_teb(), pos_(Eigen::Vector2d::Zero())
        {}

        /**
          * @brief Construct CircularObstacle using a 2d center position vector and radius
          * @param position 2d position that defines the current obstacle position
          * @param radius radius of the obstacle
          */
        CircularObstacle(const Eigen::Ref< const Eigen::Vector2d>& position, double radius) : Obstacle_teb(), pos_(position), radius_(radius)
        {}

        /**
          * @brief Construct CircularObstacle using x- and y-center-coordinates and radius
          * @param x x-coordinate
          * @param y y-coordinate
          * @param radius radius of the obstacle
          */
        CircularObstacle(double x, double y, double radius) : Obstacle_teb(), pos_(Eigen::Vector2d(x,y)), radius_(radius)
        {}


        // implements checkCollision() of the base class
        virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
        {
            return getMinimumDistance(point) < min_dist;
        }


        // implements checkLineIntersection() of the base class
        virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
        {
            // Distance Line - Circle
            // refer to http://www.spieleprogrammierer.de/wiki/2D-Kollisionserkennung#Kollision_Kreis-Strecke
            Eigen::Vector2d a = line_end-line_start; // not normalized!  a=y-x
            Eigen::Vector2d b = pos_-line_start; // b=m-x

            // Now find nearest point to circle v=x+a*t with t=a*b/(a*a) and bound to 0<=t<=1
            double t = a.dot(b)/a.dot(a);
            if (t<0) t=0; // bound t (since a is not normalized, t can be scaled between 0 and 1 to parametrize the line
            else if (t>1) t=1;
            Eigen::Vector2d nearest_point = line_start + a*t;

            // check collision
            return checkCollision(nearest_point, min_dist);
        }


        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Eigen::Vector2d& position) const
        {
            return (position-pos_).norm() - radius_;
        }

        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
        {
            return distance_point_to_segment_2d(pos_, line_start, line_end) - radius_;
        }

        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Point2dContainer& polygon) const
        {
            return distance_point_to_polygon_2d(pos_, polygon) - radius_;
        }

        // implements getMinimumDistanceVec() of the base class
        virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
        {
            return pos_ + radius_*(position-pos_).normalized();
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
        {
            return (pos_ + t*centroid_velocity_ - position).norm() - radius_;
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
        {
            return distance_point_to_segment_2d(pos_ + t*centroid_velocity_, line_start, line_end) - radius_;
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
        {
            return distance_point_to_polygon_2d(pos_ + t*centroid_velocity_, polygon) - radius_;
        }

        // implements predictCentroidConstantVelocity() of the base class
        virtual void predictCentroidConstantVelocity(double t, Eigen::Ref<Eigen::Vector2d> position) const
        {
            position = pos_ + t*centroid_velocity_;
        }

        // implements getCentroid() of the base class
        virtual const Eigen::Vector2d& getCentroid() const
        {
            return pos_;
        }

        // implements getCentroidCplx() of the base class
        virtual std::complex<double> getCentroidCplx() const
        {
            return std::complex<double>(pos_[0],pos_[1]);
        }

        // Accessor methods
        const Eigen::Vector2d& position() const {return pos_;} //!< Return the current position of the obstacle (read-only)
        Eigen::Vector2d& position() {return pos_;} //!< Return the current position of the obstacle
        double& x() {return pos_.coeffRef(0);} //!< Return the current x-coordinate of the obstacle
        const double& x() const {return pos_.coeffRef(0);} //!< Return the current y-coordinate of the obstacle (read-only)
        double& y() {return pos_.coeffRef(1);} //!< Return the current x-coordinate of the obstacle
        const double& y() const {return pos_.coeffRef(1);} //!< Return the current y-coordinate of the obstacle (read-only)
        double& radius() {return radius_;} //!< Return the current radius of the obstacle
        const double& radius() const {return radius_;} //!< Return the current radius of the obstacle

        // implements toPolygonMsg() of the base class
        virtual void toPolygonMsg(Polygon& polygon)
        {
            // TODO(roesmann): the polygon message type cannot describe a "perfect" circle
            //                 We could switch to ObstacleMsg if required somewhere...
            polygon.points.resize(1);
            polygon.points.front().x = pos_.x();
            polygon.points.front().y = pos_.y();
            polygon.points.front().z = 0;
        }

    protected:

        Eigen::Vector2d pos_; //!< Store the center position of the CircularObstacle
        double radius_ = 0.0; //!< Radius of the obstacle


    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

/**
* @class LineObstacle
* @brief Implements a 2D line obstacle
*/

    class LineObstacle : public Obstacle_teb
    {
    public:
        //! Abbrev. for a container storing vertices (2d points defining the edge points of the polygon)
        typedef std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > VertexContainer;

        /**
          * @brief Default constructor of the point obstacle class
          */
        LineObstacle() : Obstacle_teb()
        {
            start_.setZero();
            end_.setZero();
            centroid_.setZero();
        }

        /**
         * @brief Construct LineObstacle using 2d position vectors as start and end of the line
         * @param line_start 2d position that defines the start of the line obstacle
         * @param line_end 2d position that defines the end of the line obstacle
         */
        LineObstacle(const Eigen::Ref< const Eigen::Vector2d>& line_start, const Eigen::Ref< const Eigen::Vector2d>& line_end)
                : Obstacle_teb(), start_(line_start), end_(line_end)
        {
            calcCentroid();
        }

        /**
         * @brief Construct LineObstacle using start and end coordinates
         * @param x1 x-coordinate of the start of the line
         * @param y1 y-coordinate of the start of the line
         * @param x2 x-coordinate of the end of the line
         * @param y2 y-coordinate of the end of the line
         */
        LineObstacle(double x1, double y1, double x2, double y2) : Obstacle_teb()
        {
            start_.x() = x1;
            start_.y() = y1;
            end_.x() = x2;
            end_.y() = y2;
            calcCentroid();
        }

        // implements checkCollision() of the base class
        virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
        {
            return getMinimumDistance(point) <= min_dist;
        }

        // implements checkLineIntersection() of the base class
        virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
        {
            return check_line_segments_intersection_2d(line_start, line_end, start_, end_);
        }

        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Eigen::Vector2d& position) const
        {
            return distance_point_to_segment_2d(position, start_, end_);
        }

        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
        {
            return distance_segment_to_segment_2d(start_, end_, line_start, line_end);
        }

        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Point2dContainer& polygon) const
        {
            return distance_segment_to_polygon_2d(start_, end_, polygon);
        }

        // implements getMinimumDistanceVec() of the base class
        virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
        {
            return closest_point_on_line_segment_2d(position, start_, end_);
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
        {
            Eigen::Vector2d offset = t*centroid_velocity_;
            return distance_point_to_segment_2d(position, start_ + offset, end_ + offset);
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
        {
            Eigen::Vector2d offset = t*centroid_velocity_;
            return distance_segment_to_segment_2d(start_ + offset, end_ + offset, line_start, line_end);
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
        {
            Eigen::Vector2d offset = t*centroid_velocity_;
            return distance_segment_to_polygon_2d(start_ + offset, end_ + offset, polygon);
        }

        // implements getCentroid() of the base class
        virtual const Eigen::Vector2d& getCentroid() const
        {
            return centroid_;
        }

        // implements getCentroidCplx() of the base class
        virtual std::complex<double> getCentroidCplx() const
        {
            return std::complex<double>(centroid_.x(), centroid_.y());
        }

        // Access or modify line
        const Eigen::Vector2d& start() const {return start_;}
        void setStart(const Eigen::Ref<const Eigen::Vector2d>& start) {start_ = start; calcCentroid();}
        const Eigen::Vector2d& end() const {return end_;}
        void setEnd(const Eigen::Ref<const Eigen::Vector2d>& end) {end_ = end; calcCentroid();}

        // implements toPolygonMsg() of the base class
        virtual void toPolygonMsg(Polygon& polygon)
        {
            polygon.points.resize(2);
            polygon.points.front().x = start_.x();
            polygon.points.front().y = start_.y();

            polygon.points.back().x = end_.x();
            polygon.points.back().y = end_.y();
            polygon.points.back().z = polygon.points.front().z = 0;
        }

    protected:
        void calcCentroid()	{	centroid_ = 0.5*(start_ + end_); }

    private:
        Eigen::Vector2d start_;
        Eigen::Vector2d end_;

        Eigen::Vector2d centroid_;

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };


/**
* @class PillObstacle
* @brief Implements a 2D pill/stadium/capsular-shaped obstacle (line + distance/radius)
*/

    class PillObstacle : public Obstacle_teb
    {
    public:

        /**
          * @brief Default constructor of the point obstacle class
          */
        PillObstacle() : Obstacle_teb()
        {
            start_.setZero();
            end_.setZero();
            centroid_.setZero();
        }

        /**
         * @brief Construct LineObstacle using 2d position vectors as start and end of the line
         * @param line_start 2d position that defines the start of the line obstacle
         * @param line_end 2d position that defines the end of the line obstacle
         */
        PillObstacle(const Eigen::Ref< const Eigen::Vector2d>& line_start, const Eigen::Ref< const Eigen::Vector2d>& line_end, double radius)
                : Obstacle_teb(), start_(line_start), end_(line_end), radius_(radius)
        {
            calcCentroid();
        }

        /**
         * @brief Construct LineObstacle using start and end coordinates
         * @param x1 x-coordinate of the start of the line
         * @param y1 y-coordinate of the start of the line
         * @param x2 x-coordinate of the end of the line
         * @param y2 y-coordinate of the end of the line
         */
        PillObstacle(double x1, double y1, double x2, double y2, double radius) : Obstacle_teb(), radius_(radius)
        {
            start_.x() = x1;
            start_.y() = y1;
            end_.x() = x2;
            end_.y() = y2;
            calcCentroid();
        }

        // implements checkCollision() of the base class
        virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
        {
            return getMinimumDistance(point) <= min_dist;
        }

        // implements checkLineIntersection() of the base class
        virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
        {
            return check_line_segments_intersection_2d(line_start, line_end, start_, end_);
        }

        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Eigen::Vector2d& position) const
        {
            return distance_point_to_segment_2d(position, start_, end_) - radius_;
        }

        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
        {
            return distance_segment_to_segment_2d(start_, end_, line_start, line_end) - radius_;
        }

        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Point2dContainer& polygon) const
        {
            return distance_segment_to_polygon_2d(start_, end_, polygon) - radius_;
        }

        // implements getMinimumDistanceVec() of the base class
        virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
        {
            Eigen::Vector2d closed_point_line = closest_point_on_line_segment_2d(position, start_, end_);
            return  closed_point_line + radius_*(position-closed_point_line).normalized();
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
        {
            Eigen::Vector2d offset = t*centroid_velocity_;
            return distance_point_to_segment_2d(position, start_ + offset, end_ + offset) - radius_;
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
        {
            Eigen::Vector2d offset = t*centroid_velocity_;
            return distance_segment_to_segment_2d(start_ + offset, end_ + offset, line_start, line_end) - radius_;
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
        {
            Eigen::Vector2d offset = t*centroid_velocity_;
            return distance_segment_to_polygon_2d(start_ + offset, end_ + offset, polygon) - radius_;
        }

        // implements getCentroid() of the base class
        virtual const Eigen::Vector2d& getCentroid() const
        {
            return centroid_;
        }

        // implements getCentroidCplx() of the base class
        virtual std::complex<double> getCentroidCplx() const
        {
            return std::complex<double>(centroid_.x(), centroid_.y());
        }

        // Access or modify line
        const Eigen::Vector2d& start() const {return start_;}
        void setStart(const Eigen::Ref<const Eigen::Vector2d>& start) {start_ = start; calcCentroid();}
        const Eigen::Vector2d& end() const {return end_;}
        void setEnd(const Eigen::Ref<const Eigen::Vector2d>& end) {end_ = end; calcCentroid();}

        // implements toPolygonMsg() of the base class
        virtual void toPolygonMsg(Polygon& polygon)
        {
            // Currently, we only export the line
            // TODO(roesmann): export whole pill
            polygon.points.resize(2);
            polygon.points.front().x = start_.x();
            polygon.points.front().y = start_.y();

            polygon.points.back().x = end_.x();
            polygon.points.back().y = end_.y();
            polygon.points.back().z = polygon.points.front().z = 0;
        }

    protected:
        void calcCentroid()    {    centroid_ = 0.5*(start_ + end_); }

    private:
        Eigen::Vector2d start_;
        Eigen::Vector2d end_;
        double radius_ = 0.0;

        Eigen::Vector2d centroid_;

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

/**
 * @class PolygonObstacle
 * @brief Implements a polygon obstacle with an arbitrary number of vertices
 * @details If the polygon has only 2 vertices, than it is considered as a line,
 * 	    otherwise the polygon will always be closed (a connection between the first and the last vertex
 * 	    is included automatically).
 */
    class PolygonObstacle : public Obstacle_teb
    {
    public:

        /**
          * @brief Default constructor of the polygon obstacle class
          */
        PolygonObstacle() : Obstacle_teb(), finalized_(false)
        {
            centroid_.setConstant(NAN);
        }

        /**
         * @brief Construct polygon obstacle with a list of vertices
         */
        PolygonObstacle(const Point2dContainer& vertices) : Obstacle_teb(), vertices_(vertices)
        {
            finalizePolygon();
        }


        /* FIXME Not working at the moment due to the aligned allocator version of std::vector
          * And it is C++11 code that is disabled atm to ensure compliance with ROS indigo/jade
        template <typename... Vector2dType>
        PolygonObstacle(const Vector2dType&... vertices) : _vertices({vertices...})
        {
          calcCentroid();
          _finalized = true;
        }
        */


        // implements checkCollision() of the base class
        virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
        {
            // line case
            if (noVertices()==2)
                return getMinimumDistance(point) <= min_dist;

            // check if point is in the interior of the polygon
            // point in polygon test - raycasting (http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html)
            // using the following algorithm we may obtain false negatives on edge-cases, but that's ok for our purposes
            int i, j;
            bool c = false;
            for (i = 0, j = noVertices()-1; i < noVertices(); j = i++)
            {
                if ( ((vertices_.at(i).y()>point.y()) != (vertices_.at(j).y()>point.y())) &&
                     (point.x() < (vertices_.at(j).x()-vertices_.at(i).x()) * (point.y()-vertices_.at(i).y()) / (vertices_.at(j).y()-vertices_.at(i).y()) + vertices_.at(i).x()) )
                    c = !c;
            }
            if (c>0) return true;

            // If this statement is reached, the point lies outside the polygon or maybe on its edges
            // Let us check the minium distance as well
            return min_dist == 0 ? false : getMinimumDistance(point) < min_dist;
        }


        /**
          * @brief Check if a given line segment between two points intersects with the obstacle (and additionally keeps a safty distance \c min_dist)
          * @param line_start 2D point for the end of the reference line
          * @param line_end 2D point for the end of the reference line
          * @param min_dist Minimum distance allowed to the obstacle to be collision/intersection free
          * @remarks we ignore \c min_dist here
          * @return \c true if given line intersects the region of the obstacle or if the minimum distance is lower than min_dist
          */
        virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const;


        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Eigen::Vector2d& position) const
        {
            return distance_point_to_polygon_2d(position, vertices_);
        }

        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
        {
            return distance_segment_to_polygon_2d(line_start, line_end, vertices_);
        }

        // implements getMinimumDistance() of the base class
        virtual double getMinimumDistance(const Point2dContainer& polygon) const
        {
            return distance_polygon_to_polygon_2d(polygon, vertices_);
        }

        // implements getMinimumDistanceVec() of the base class
        virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const;

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
        {
            Point2dContainer pred_vertices;
            predictVertices(t, pred_vertices);
            return distance_point_to_polygon_2d(position, pred_vertices);
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
        {
            Point2dContainer pred_vertices;
            predictVertices(t, pred_vertices);
            return distance_segment_to_polygon_2d(line_start, line_end, pred_vertices);
        }

        // implements getMinimumSpatioTemporalDistance() of the base class
        virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
        {
            Point2dContainer pred_vertices;
            predictVertices(t, pred_vertices);
            return distance_polygon_to_polygon_2d(polygon, pred_vertices);
        }

        virtual void predictVertices(double t, Point2dContainer& pred_vertices) const
        {
            // Predict obstacle (polygon) at time t
            pred_vertices.resize(vertices_.size());
            Eigen::Vector2d offset = t*centroid_velocity_;
            for (std::size_t i = 0; i < vertices_.size(); i++)
            {
                pred_vertices[i] = vertices_[i] + offset;
            }
        }

        // implements getCentroid() of the base class
        virtual const Eigen::Vector2d& getCentroid() const
        {
            assert(finalized_ && "Finalize the polygon after all vertices are added.");
            return centroid_;
        }

        // implements getCentroidCplx() of the base class
        virtual std::complex<double> getCentroidCplx() const
        {
            assert(finalized_ && "Finalize the polygon after all vertices are added.");
            return std::complex<double>(centroid_.coeffRef(0), centroid_.coeffRef(1));
        }

        // implements toPolygonMsg() of the base class
        virtual void toPolygonMsg(Polygon& polygon);


        /** @name Define the polygon */
        ///@{

        // Access or modify polygon
        const Point2dContainer& vertices() const {return vertices_;} //!< Access vertices container (read-only)
        Point2dContainer& vertices() {return vertices_;} //!< Access vertices container

        /**
          * @brief Add a vertex to the polygon (edge-point)
          * @remarks You do not need to close the polygon (do not repeat the first vertex)
          * @warning Do not forget to call finalizePolygon() after adding all vertices
          * @param vertex 2D point defining a new polygon edge
          */
        void pushBackVertex(const Eigen::Ref<const Eigen::Vector2d>& vertex)
        {
            vertices_.push_back(vertex);
            finalized_ = false;
        }

        /**
          * @brief Add a vertex to the polygon (edge-point)
          * @remarks You do not need to close the polygon (do not repeat the first vertex)
          * @warning Do not forget to call finalizePolygon() after adding all vertices
          * @param x x-coordinate of the new vertex
          * @param y y-coordinate of the new vertex
          */
        void pushBackVertex(double x, double y)
        {
            vertices_.push_back(Eigen::Vector2d(x,y));
            finalized_ = false;
        }

        /**
          * @brief Call finalizePolygon after the polygon is created with the help of pushBackVertex() methods
          */
        void finalizePolygon()
        {
            fixPolygonClosure();
            calcCentroid();
            finalized_ = true;
        }

        /**
          * @brief Clear all vertices (Afterwards the polygon is not valid anymore)
          */
        void clearVertices() {vertices_.clear(); finalized_ = false;}

        /**
          * @brief Get the number of vertices defining the polygon (the first vertex is counted once)
          */
        int noVertices() const {return (int)vertices_.size();}


        ///@}

    protected:

        void fixPolygonClosure(); //!< Check if the current polygon contains the first vertex twice (as start and end) and in that case erase the last redundant one.

        void calcCentroid(); //!< Compute the centroid of the polygon (called inside finalizePolygon())


        Point2dContainer vertices_; //!< Store vertices defining the polygon (@see pushBackVertex)
        Eigen::Vector2d centroid_; //!< Store the centroid coordinates of the polygon (@see calcCentroid)

        bool finalized_; //!< Flat that keeps track if the polygon was finalized after adding all vertices


    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

} // namespace teb_local_planner
#endif //TEB_OBSTACLES_H
